#include<SoftwareSerial.h>

#define LEDPIN 3
#define BUTTONPIN 4
#define TRXRATE 19200
#define TERMBAUD 9600
#define RX_1 5
#define TX_1 6
#define RX_2 8
#define TX_2 9
#define RX_3 10
#define TX_3 11
#define RX_4 12
#define TX_4 13

#define COM 170
#define ROGER 171
#define NEGATIVE 172
#define SETID 173
#define SENDID 174
#define GETNEIGHBOR 175
#define SETNEIGHBOR 176
#define NOCOM 255

SoftwareSerial mySerial_1(RX_1,TX_1);
SoftwareSerial mySerial_2(RX_2,TX_2);
SoftwareSerial mySerial_3(RX_3,TX_3);
SoftwareSerial mySerial_4(RX_4,TX_4);

uint8_t myID = uint8_t(-1);
int numOfNeighbors = 1; //There must always be one!
uint8_t neighbors[4];
uint8_t* read_buffer;
uint8_t write_buffer[64];

//https://stackoverflow.com/questions/15094834/check-if-a-value-exist-in-a-array
int isvalueinarray(uint8_t val, uint8_t *arr, int size){
    int i;
    for (i=0; i < size; i++) {
        if (arr[i] == val)
            return i;
    }
    return -1;
}

void listenToChannel(int channel){
	switch(channel) {
		case 1:
			mySerial_1.listen();
			break;
		case 2:
			mySerial_2.listen();
			break;
		case 3:
			mySerial_3.listen();
			break;
		case 4:
			mySerial_4.listen();
			break;
		default:
			Serial.println("Invalid channel selection for listenToChannel(int channel)");
			Serial.print("Error! Cannot listen on channel ");
			Serial.println(channel);		
	}
}

void checkForData(int channel, uint8_t code) {
	Serial.println("Checking for data...");
	if(code != NOCOM) {
		Serial.println("Data found!");
		lookForValidData(channel, code);
	} else {
		Serial.print("No data found on channel ");
		Serial.println(channel);
	}
}

void lookForValidData(int channel, uint8_t code) {
	Serial.println("Checking if data is valid...");
	if(code == COM) {
		Serial.print("Com established on channel ");
		Serial.println(channel);
		if(channel != 255) {
			read_buffer = readSerialToBuffer(channel);
			checkBuffer(channel, read_buffer);
		} else {
			char temp_buf_1[16];
			char temp_buf_2[16];
			uint8_t temp_buf_3[16];
			Serial.readBytes(temp_buf_1, 6);
			temp_buf_2[0] = temp_buf_1[0]; 
			temp_buf_2[1] = temp_buf_1[1]; 
			int num;
			num = atoi(temp_buf_2);
			temp_buf_3[0] = uint8_t(num);
			temp_buf_2[0] = temp_buf_1[2]; 
			temp_buf_2[1] = temp_buf_1[3];
			temp_buf_2[2] = temp_buf_1[4];
			num = atoi(temp_buf_2);
			temp_buf_3[1] = uint8_t(num);
			temp_buf_2[0] = temp_buf_1[5]; 
			temp_buf_2[1] = 0;
			temp_buf_2[2] = 0;
			num = atoi(temp_buf_2);
			temp_buf_3[2] = uint8_t(num);
			read_buffer = temp_buf_3;
			int i = 0;
			for(i; i < 3; ++i){
				Serial.println(read_buffer[i]); 
			}
			checkBuffer(channel, read_buffer); 
		}	
	} else {
		Serial.print("Data invalid, checking again on channel ");
		Serial.println(channel);
		switch(channel) {
			case 1:
				lookForValidData(channel, mySerial_1.read());
				break;
			case 2:
				lookForValidData(channel, mySerial_2.read());
				break;
			case 3:
				lookForValidData(channel, mySerial_3.read());
				break;
			case 4:
				lookForValidData(channel, mySerial_4.read());
				break;
			case 255:
				if(Serial.available() >0) {
				  char buf[3];
				  Serial.readBytes(buf, 3);
				  int num = atoi(buf);
				  uint8_t data = uint8_t(num);
				  lookForValidData(channel, data);
				}
				break;
			default:
				Serial.println("Invalid channel selection for lookForValidData(int channel, uint8_t code)");
				Serial.print("Error! Cannot check for valid data on channel ");
				Serial.println(channel);	
				break;
		}
	}
}

void checkBuffer(int channel, uint8_t *arr) {
	Serial.println("Checking data in read buffer...");
	if(arr[0] <= 16) {
        Serial.println("Data is for me! Looking up code...");
		int code = arr[0];
		lookUpCode(channel, code);
	} else {
        Serial.print("Data is not for me! Sending it to neighbor on channel ");
		int neighbor = arr[0];
        neighbor = isvalueinarray(neighbor, neighbors, 4);
        arr[0] = COM;
		switch(neighbor) {
			case 1:
                Serial.println("1");
				writeBufferToSerial(1, arr, sizeof(arr)/sizeof(uint8_t));
				break;
			case 2:
                Serial.println("2");
				writeBufferToSerial(2, arr, sizeof(arr)/sizeof(uint8_t));
				break;
			case 3:
                Serial.println("3");
				writeBufferToSerial(3, arr, sizeof(arr)/sizeof(uint8_t));
				break;
			case 4:
                Serial.println("4");
				writeBufferToSerial(4, arr, sizeof(arr)/sizeof(uint8_t));
				break;
			case 255:
				Serial.println("255");
				int i = 0;
				for(i; i < sizeof(arr)/sizeof(arr[0]); ++i){
				 Serial.println(arr[i]); 
				}
				break;
		}
	}
}

void lookUpCode(int channel, uint8_t code) {
	switch(code){
		case SETID:
			setMyID(channel);
            break;
		case SENDID:
            sendMyID(channel);
			break;
		case GETNEIGHBOR:
			
			break;
		case SETNEIGHBOR:
			
			break;
		default:
			Serial.println("Invalid code for lookUpCode(int channel, uint8_t code)");
			Serial.print("Error! Invalid code from channel ");
			Serial.println(channel);
			break;
	}
}

uint8_t* readSerialToBuffer(int channel) {
	uint8_t data[64];
	int i = 0;
	switch(channel){
		case 1:
			while(mySerial_1.peek() != -1){
				data[i] = mySerial_1.read();
				i += 1;
			}
			break;
		case 2:
			while(mySerial_2.peek() != -1){
				data[i] = mySerial_2.read();
				i += 1;
			}
			break;
		case 3:
			while(mySerial_3.peek() != -1){
				data[i] = mySerial_3.read();
				i += 1;
			}
			break;
		case 4:
			while(mySerial_4.peek() != -1){
				data[i] = mySerial_4.read();
				i += 1;
			}
			break;
		default:
			Serial.println("Invalid channel selection for readSerialToBuffer(int channel)");
			Serial.print("Error! cannot read serial channel ");
			Serial.println(channel);
			break;
		}
		return data;
}

void writeBufferToSerial(int channel, uint8_t *arr, int buf_size) {
  switch(channel){
    case 1:
      mySerial_1.write(arr, buf_size);
      break;
    case 2:
      mySerial_2.write(arr, buf_size);
      break;
    case 3:
      mySerial_3.write(arr, buf_size);
      break;
    case 4:
      mySerial_4.write(arr, buf_size);
      break;
    default:
      Serial.println("Invalid channel selection for writebuffertoserial()");
      Serial.print("Error! cannot write to serial channel ");
      Serial.println(channel);
      break;
    }
}

void setMyID(int channel) {
	Serial.println("Setting new ID...");
	myID = read_buffer[1];
	Serial.print("New ID set! New ID is ");
	Serial.println(read_buffer[1]);
	write_buffer[0] = COM;
	write_buffer[1] = ROGER;
	if(channel != 255){
		int i = 0;
		for(i; i < 10; ++i) {
			writeBufferToSerial(channel, write_buffer, 2); //sends the ROGER code back to the neighbor that sent the SETID code.
		}
	} else {
		int i = 0;
		for(i; i < 2; ++i) {
		Serial.println(write_buffer[i]); 
		}
	}
}

void sendMyID(int channel) {
	Serial.print("Sending my ID to channel ");
	Serial.println(channel);
	int i;
	write_buffer[0] = COM;
	write_buffer[1] = myID;
	for(i; i < 10; ++i) {
		writeBufferToSerial(channel, write_buffer, 2);
	}
}

void setNeighbor(int channel) {
	listenToChannel(channel);
	write_buffer[0] = COM;
	write_buffer[1] = SETID;
	write_buffer[2] = myID + numOfNeighbors;
	int i = 0;
	for(i; i < 10; ++i) {
		writeBufferToSerial(channel, write_buffer, 3);
	}
	i = 0;
	switch(channel) {
		case 1:
			for(i; i < 20; ++i){
				if(mySerial_1.read() != COM) {
					Serial.print("Waiting for a response on channel ");
					Serial.println(channel);
				} else {
					read_buffer = readSerialToBuffer(channel);
					break;
				}
			}
			break;
		case 2:
			for(i; i < 20; ++i){
				if(mySerial_1.read() != COM) {
					Serial.print("Waiting for a response on channel ");
					Serial.println(channel);
				} else {
					read_buffer = readSerialToBuffer(channel);
					break;
				}
			}
			break;
		case 3:
			for(i; i < 20; ++i){
				if(mySerial_1.read() != COM) {
					Serial.print("Waiting for a response on channel ");
					Serial.println(channel);
				} else {
					read_buffer = readSerialToBuffer(channel);
					break;
				}
			}
			break;
		case 4:
			for(i; i < 20; ++i){
				if(mySerial_1.read() != COM) {
					Serial.print("Waiting for a response on channel ");
					Serial.println(channel);
				} else {
					read_buffer = readSerialToBuffer(channel);
					break;
				}
			}
			break;
	}
	if(i != 20){
		if(read_buffer[0] == ROGER) {
			Serial.print("New ID was assigned on channel ");
			Serial.println(channel);
			numOfNeighbors = numOfNeighbors + 1;
		} else {
			Serial.print("Neighbor on channel ");
			Serial.print(channel);
			Serial.println(" already has an ID!");
		}
	} else {
		Serial.print("No neighbor detected on channel ");
		Serial.println(channel);
	}
}

void setup() {
	Serial.begin(TERMBAUD);
	Serial.println("Attempting to initialize serial coms...");
	Serial.println("...");
	pinMode(RX_1, INPUT);
	pinMode(TX_1, OUTPUT);
	pinMode(RX_2, INPUT);
	pinMode(TX_2, OUTPUT);
	pinMode(RX_3, INPUT);
	pinMode(TX_3, OUTPUT);
	pinMode(RX_4, INPUT);
	pinMode(TX_4, OUTPUT);
	mySerial_1.begin(TRXRATE);
	Serial.println("...");
	mySerial_2.begin(TRXRATE);
	Serial.println("...");
	mySerial_3.begin(TRXRATE);
	Serial.println("...");
	mySerial_4.begin(TRXRATE);
	Serial.println("...");
	pinMode(LEDPIN, OUTPUT);
	Serial.println("...initialization complete.");
	digitalWrite(LEDPIN, LOW);
}

void loop() {
	if(Serial.available() >0) {
	  char buf[3];
	  Serial.readBytes(buf, 3);
	  int num = atoi(buf);
	  uint8_t data = uint8_t(num);
	  Serial.print("Data found on serial monitor! The data was ");
	  Serial.println(data);
	  checkForData(255, data);
	}
	listenToChannel(1);
	Serial.println("Listening on channel 1...");
	checkForData(1, mySerial_1.read());
	listenToChannel(2);
	Serial.println("Listening on channel 2...");
	checkForData(2, mySerial_2.read());
	listenToChannel(3);
	Serial.println("Listening on channel 3...");
	checkForData(3, mySerial_3.read());
	listenToChannel(4);
	Serial.println("Listening on channel 4...");
	checkForData(4, mySerial_4.read());
}
